#include "Gyro.hpp"

int ZeroDrift_gyro_z=0;

int16_t FJ_gyro_z = 0;
volatile float FJ_Angle = 0;//最后引出的变量
volatile float FJ_LastAngleSpeed = 0,FJ_AngleSpeed = 0;
volatile float FJ_Angle_Max =  180;
volatile float FJ_Angle_Min = -180;
volatile float AngleSpeed = 0;

extern volatile int Ramp_Flag;//坡道标志

void Zero_Point_Detect(void)
{
    uint8_t i;
    int zero_point_y_accu=0;
    int zero_point_z_accu=0;
    for(i=0;i<=100;i++)//积累100次，求取平均值，获取当前零飘
    {zero_point_z_accu+=Gyro_value(5);
        Delay_Ms(3);}
    ZeroDrift_gyro_z=zero_point_z_accu/100.0;
}

void Gyroscope_GetData(void)
{
    int16_t gyro_z =0;
    gyro_z = Gyro_value(5);
    AngleSpeed = ((gyro_z-ZeroDrift_gyro_z) * GYRO_SENS)*DT;
}

//-------------------------------------------------------------------------------------------------------------------
//  @brief      陀螺仪计算航偏角角度
//  @param
//  @return     void
//-------------------------------------------------------------------------------------------------------------------
void Get_Gyroscope_Angle(void)
{
    float K=0.7;
    FJ_gyro_z = Gyro_value(5);
    FJ_LastAngleSpeed=FJ_AngleSpeed;
    FJ_AngleSpeed += ((FJ_gyro_z-ZeroDrift_gyro_z) * GYRO_SENS)*DT;
    FJ_Angle = FJ_AngleSpeed*K+FJ_LastAngleSpeed*(1-K);                //向左为正
    FJ_Angle = FJ_Angle > FJ_Angle_Max ? FJ_Angle_Max : FJ_Angle;
    FJ_Angle = FJ_Angle < FJ_Angle_Min ? FJ_Angle_Min : FJ_Angle;
    FJ_Angle = -FJ_Angle;
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      YAW
//  @param
//  @return     void
//  @note
//-------------------------------------------------------------------------------------------------------------------
void Clear_Gyroscope_Angle(void)
{
    FJ_Angle = 0;
    FJ_gyro_z = 0;
    FJ_AngleSpeed = 0;
    FJ_LastAngleSpeed = 0;
}